#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File         :formation_controller.py
@Description  :
@Time         :2024/05/21 09:38:44
@Author       :Lin Yuheng
@Version      :1.0
'''

import numpy as np
import math

class Vector:
    def __init__(self, x=0, y=0):
        self.x = x
        self.y = y
    
    def add(self, other:"Vector"):
        return Vector(self.x + other.x, self.y + other.y)
    
    def sub(self, other:"Vector"):
        return Vector(self.x - other.x, self.y - other.y)
    
    def mul(self, other:"Vector"):
        return Vector(self.x * other, self.y * other)
    
    def div(self, other:"Vector"):
        return Vector(self.x / other, self.y / other)
    
    def reverse(self):
        return Vector(-self.x, -self.y)
    
    def pow(self, other:"Vector"):
        return Vector(self.x ** other, self.y ** other)
    
    def dot(self, other:"Vector"):
        """ 点乘 """
        return self.x * other.x + self.y * other.y
    
    def cross(self, other:"Vector"):
        """ 叉乘 """
        return self.x * other.y - self.y * other.x
    
    def Angle(self, other:"Vector"):
        return math.acos(self.dot(other) / (self.len() * other.norm()))

    def Area(self, other:"Vector"):
        return math.fabs(self.cross(other))
    
    # 向量旋转
    # 旋转公式
    #   Nx     (cos  -sin) x
    #   Ny     (sin   cos) y
    def Rotate(self, rad):
        return Vector(self.x * math.cos(rad) - self.y * math.sin(rad), self.x * math.sin(rad) + self.y * math.cos(rad))
    
    # 返回向量的法向量
    def Normal(self):
        return Vector(-self.y, self.x)
    
    def len(self):
        return math.sqrt(self.x ** 2 + self.y ** 2)
    
    def len2(self):
        return self.x ** 2 + self.y ** 2
    
    def normlized(self):
        if self.len() == 0:
            return Vector(0,0)
        else:
            return Vector(self.x / self.len(), self.y / self.len())
    
    def dist(self, other:"Vector"):
        return math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2)
    
    def print(self):
        print("x:", self.x, "y:", self.y)

    def set_vec_ele(self, a:float, b:float):
        self.x = a
        self.y = b

class Point(Vector):
    def __init__(self, x=0, y=0):
        super().__init__(x, y)